//----------------------------------------------
//				main.c
// 	Simple IMU
// 	Arthur Bemann 07/10/2011
//-----------------------------------------------
//
//	Description:
//	Uses a buspirate and a 9dof stick  sparkfun boards, 
//	to make a simple IMU
//	
//-----------------------------------------------
//	
//	Requeriments:
//  2 - RP pins
//  1 - I2C module
//
//-----------------------------------------------
#include <stdio.h>

#include "Compiler.h"
#include "HardwareProfile.h"
#include "SERIAL/UART2.h"
#include "I2C/I2C.h"
#include "ITG3200/ITG3200.h"
#include "ADXL345/ADXL345.h"
#include "DELAY/Delay.h"

//-------------------- CONFIGURATION --------------------
//set custom configuration for PIC 24F
_CONFIG2(FNOSC_FRCPLL & OSCIOFNC_ON &POSCMOD_NONE & I2C1SEL_PRI)			// Fast RC oscillator w/ divide and PLL
_CONFIG1(JTAGEN_OFF & GCP_OFF & GWRP_OFF & COE_OFF & FWDTEN_OFF & ICS_PGx1) // turn off junk we don't need


//--------------------- PRIVATE PROTOTYPES ---------------
void initSystem(void);
void Clock32MHz(void);
void initIO(void);

// ---------------- MAIN --------------------------------------
int main(void)
{
	int Gyro[4],Accel[3];

	initSystem();

	while(1)
	{
		LED_ON();

        readAccel(Accel);
        readGyro(Gyro);

		printf("Data");
		putU2(Accel[0]&0xff);
		putU2(Accel[0]>>8);
		putU2(Accel[1]&0xff);
		putU2(Accel[1]>>8);
		putU2(Accel[2]&0xff);
		putU2(Accel[2]>>8);

		putU2(Gyro[1]&0xff);
		putU2(Gyro[1]>>8);
		putU2(Gyro[2]&0xff);
		putU2(Gyro[2]>>8);
		putU2(Gyro[3]&0xff);
		putU2(Gyro[3]>>8);
		putU2(Gyro[0]&0xff);
		putU2(Gyro[0]>>8);

		lineU2();


		Delayms(10);
		LED_OFF();
		Delayms(10);

	}//while
}


//-------------- initSystem -----------------------------------
// Initilize the microcontroler, startup of all modules
// ( audio, video, serial,keyboard, ...)
void initSystem(void)
{
	extern int __C30_UART; __C30_UART = 2; 

	Clock32MHz();
	initIO();
	initU2();
	initI2C();
	VREG_ON();

	Delayms(50);	

	putsU2("picIMU v0 ");
	putsU2(__DATE__);

	if(initGyro())
		{printf("Error on Gyro initialization");while(1);}
	if(initAccel())
		{printf("Error on Accel initialization");while(1);}
}

//-------------- initIO -----------------------------------------
// Initialize all general porpuse I/O pins
void initIO(void)
{
	// Configure digital/analog pins
	AD1PCFG = 0xffff;

	// Configure Led pin
	LED_DIR = 0;
}

//-------------- Clock32MHz -------------------------------------
// Init System clock to FRC OSC with PLL,  8MHz*4 = 32MHz
void Clock32MHz(void)
{
	CLKDIVbits.RCDIV =0; //clock divider to 0
	OSCCONbits.SOSCEN=0;
}



